#ifndef __SENSORS_READ_H__
#define __SENSORS_READ_H__

#include <xqueue.h>
#include <xtask.h>
#include <rrcon.h>

namespace sensors_test {
    using namespace kthrobot;

    typedef XQueue<ADCRawData> SensorsDataQueue;
    typedef XQueue<int> AnglesQueue;

    class SensorsReader : public XTask {
    public:
        SensorsReader();
        virtual ~SensorsReader();

        void TaskCode();

        SensorsDataQueue* get_queue() { return &data_queue_; }
        AnglesQueue* get_angles_queue() { return &angles_queue_; }

        static int ConvertToCm(int value, int type);

    private:
        SensorsDataQueue data_queue_;
        AnglesQueue angles_queue_;
        static int ir_calibration_function_(int V, int k, int m_prime, int b_prime);
        static int raw_to_angle_(int raw_value);

        static const int period_ = 200; // ms
        static const int gyro_offset_ = 940;
        static const int gyro_treshold_ = 20;
        static const int gyro_scale_ = 1833;
    };
};

#endif // __SENSORS_READ_H__
